
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_gps_backend.c
  * @author     baiyang
  * @date       2022-2-13
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <sys/time.h>
#include <stdio.h>


#include "sensor_gps.h"
#include "sensor_gps_backend.h"

#include <rtc/gp_rtc.h>
#include <gcs_mavlink/gcs.h>
#include <common/gp_defines.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/
#define GPS_BACKEND_DEBUGGING 0

#if GPS_BACKEND_DEBUGGING
#define GPS_BACKEND_DEUBG(fmt, args ...)  do {console_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); rt_thread_mdelay(1);} while(0)
#else
#define GPS_BACKEND_DEUBG(fmt, args ...)
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void _detection_message(sensor_gps_backend_t gps_backend, char *buffer, uint8_t buflen);
static void _bind_function(sensor_gps_backend_t gps_backend, struct sensor_gps_backend_ops* ops);
static void inject_data(sensor_gps_backend_t gps_backend, const uint8_t *data, uint16_t len);
static void send_mavlink_gps_rtk(sensor_gps_backend_t gps_backend, mavlink_channel_t chan);
static void Write_AP_Logger_Log_Startup_messages(sensor_gps_backend_t gps_backend);
/*----------------------------------variable----------------------------------*/
static struct sensor_gps_backend_ops gps_backend_ops = {.gps_backend_destructor = NULL,
                                                        .read                   = NULL,
                                                        .highest_supported_status = NULL,
                                                        .is_configured          = NULL,
                                                        .inject_data            = inject_data,
                                                        .supports_mavlink_gps_rtk_message = NULL,
                                                        .send_mavlink_gps_rtk   = send_mavlink_gps_rtk,
                                                        .broadcast_configuration_failure_reason = NULL,
                                                        .handle_msg             = NULL,
                                                        .get_lag                = NULL,
                                                        .is_healthy             = NULL,
                                                        .logging_healthy        = NULL,
                                                        .name                   = NULL,
                                                        .Write_AP_Logger_Log_Startup_messages = Write_AP_Logger_Log_Startup_messages,
                                                        .prepare_for_arming     = NULL,
                                                        .get_RTCMV3             = NULL,
                                                        .clear_RTCMV3           = NULL,
                                                        .get_error_codes        = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_gps_backend_ctor(sensor_gps_backend_t gps_backend, struct sensor_gps_backend_ops* ops, struct GPS_State *_state, rt_device_t _port)
{
    gps_backend->port = _port;
    gps_backend->state = _state;

    gps_backend->state->have_speed_accuracy = false;
    gps_backend->state->have_horizontal_accuracy = false;
    gps_backend->state->have_vertical_accuracy = false;

    jitter_correction(&gps_backend->jitter_correction, 500, 100);

    _bind_function(gps_backend, ops);
}

int32_t sensor_gps_backend_swap_int32(int32_t v)
{
    const uint8_t *b = (const uint8_t *)&v;
    union {
        int32_t v;
        uint8_t b[4];
    } u;

    u.b[0] = b[3];
    u.b[1] = b[2];
    u.b[2] = b[1];
    u.b[3] = b[0];

    return u.v;
}

int16_t sensor_gps_backend_swap_int16(int16_t v)
{
    const uint8_t *b = (const uint8_t *)&v;
    union {
        int16_t v;
        uint8_t b[2];
    } u;

    u.b[0] = b[1];
    u.b[1] = b[0];

    return u.v;
}

/*
  fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
 */
void sensor_gps_backend_fill_3d_velocity(sensor_gps_backend_t gps_backend)
{
    float gps_heading = radians(gps_backend->state->ground_course);

    gps_backend->state->velocity.x = gps_backend->state->ground_speed * cosf(gps_heading);
    gps_backend->state->velocity.y = gps_backend->state->ground_speed * sinf(gps_heading);
    gps_backend->state->velocity.z = 0;
    gps_backend->state->have_vertical_velocity = false;
}

/*
   fill in time_week_ms and time_week from BCD date and time components
   assumes MTK19 millisecond form of bcd_time
*/
void sensor_gps_backend_make_gps_time(sensor_gps_backend_t gps_backend, uint32_t bcd_date, uint32_t bcd_milliseconds)
{
    struct tm tm = {0};

    tm.tm_year = 100U + bcd_date % 100U;
    tm.tm_mon  = ((bcd_date / 100U) % 100U)-1;
    tm.tm_mday = bcd_date / 10000U;

    uint32_t v = bcd_milliseconds;
    uint16_t msec = v % 1000U; v /= 1000U;
    tm.tm_sec = v % 100U; v /= 100U;
    tm.tm_min = v % 100U; v /= 100U;
    tm.tm_hour = v % 100U;

    // convert from time structure to unix time
    time_t unix_time = rtc_mktime(&tm);

    // convert to time since GPS epoch
    const uint32_t unix_to_GPS_secs = 315964800UL;
    const uint16_t leap_seconds_unix = GPS_LEAPSECONDS_MILLIS/1000U;
    uint32_t ret = unix_time + leap_seconds_unix - unix_to_GPS_secs;

    // get GPS week and time
    gps_backend->state->time_week = ret / GP_SEC_PER_WEEK;
    gps_backend->state->time_week_ms = (ret % GP_SEC_PER_WEEK) * GP_MSEC_PER_SEC;
    gps_backend->state->time_week_ms += msec;
}

/*
  set a timestamp based on arrival time on uart at current byte,
  assuming the message started nbytes ago
 */
void sensor_gps_backend_set_uart_timestamp(sensor_gps_backend_t gps_backend, uint16_t nbytes)
{
    if (gps_backend->port) {
        gps_backend->state->uart_timestamp_ms = SerialManager_receive_time_constraint_us(gps_backend->port, nbytes) / 1000U;
    }
}

void sensor_gps_backend_check_new_itow(sensor_gps_backend_t gps_backend, uint32_t itow, uint32_t msg_length)
{
    if (itow != gps_backend->_last_itow) {
        gps_backend->_last_itow = itow;

        /*
          we need to calculate a pseudo-itow, which copes with the
          iTow from the GPS changing in unexpected ways. We assume
          that timestamps from the GPS are always in multiples of
          50ms. That means we can't handle a GPS with an update rate
          of more than 20Hz. We could do more, but we'd need the GPS
          poll time to be higher
         */
        const uint32_t gps_min_period_ms = 50;

        // get the time the packet arrived on the UART
        uint64_t uart_us;
        if (gps_backend->port) {
            uart_us = SerialManager_receive_time_constraint_us(gps_backend->port, msg_length);
        } else {
            uart_us = time_micros64();
        }

        uint32_t now = time_millis();
        uint32_t dt_ms = now - gps_backend->_last_ms;
        gps_backend->_last_ms = now;

        // round to nearest 50ms period
        dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;

        // work out an actual message rate. If we get 5 messages in a
        // row with a new rate we switch rate
        if (gps_backend->_last_rate_ms == dt_ms) {
            if (gps_backend->_rate_counter < 5) {
                gps_backend->_rate_counter++;
            } else if (gps_backend->_rate_ms != dt_ms) {
                gps_backend->_rate_ms = dt_ms;
            }
        } else {
            gps_backend->_rate_counter = 0;
            gps_backend->_last_rate_ms = dt_ms;
        }
        if (gps_backend->_rate_ms == 0) {
            // only allow 5Hz to 20Hz in user config
            gps_backend->_rate_ms = math_constrain_int16(sensor_gps_get_rate_ms(gps_backend->state->instance), 50, 200);
        }

        // round to calculated message rate
        dt_ms = ((dt_ms + (gps_backend->_rate_ms/2)) / gps_backend->_rate_ms) * gps_backend->_rate_ms;

        // calculate pseudo-itow
        gps_backend->_pseudo_itow += dt_ms * 1000U;

        // use msg arrival time, and correct for jitter
        uint64_t local_us = jitter_correct_offboard_timestamp_usec(&gps_backend->jitter_correction, gps_backend->_pseudo_itow, uart_us);
        gps_backend->state->uart_timestamp_ms = local_us / 1000U;

        // look for lagged data from the GPS. This is meant to detect
        // the case that the GPS is trying to push more data into the
        // UART than can fit (eg. with GPS_RAW_DATA at 115200).
        float expected_lag;
        if (sensor_gps_get_lag(gps_backend->state->instance, &expected_lag)) {
            float lag_s = (now - gps_backend->state->uart_timestamp_ms) * 0.001;
            if (lag_s > expected_lag+0.05) {
                // more than 50ms over expected lag, increment lag counter
                gps_backend->state->lagged_sample_count++;
            } else {
                gps_backend->state->lagged_sample_count = 0;
            }
        }
    }
}

/*
  access to driver option bits
 */
enum GpsDriverOptions sensor_gps_backend_driver_options(void)
{
    return (enum GpsDriverOptions)(gps._driver_options);
}

// get GPS type, for subtype config
enum GPS_Type sensor_gps_backend_get_type(sensor_gps_backend_t gps_backend)
{
    return sensor_gps_get_type(gps_backend->state->instance);
}

void sensor_gps_backend_broadcast_gps_type(sensor_gps_backend_t gps_backend)
{
    char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
    _detection_message(gps_backend, buffer, sizeof(buffer));
    gcs_send_text(MAV_SEVERITY_INFO, "%s", buffer);

    GPS_BACKEND_DEUBG("%s", buffer);
}

///
static void _detection_message(sensor_gps_backend_t gps_backend, char *buffer, uint8_t buflen)
{
    const uint8_t instance = gps_backend->state->instance;
    const struct gps_detect_state* dstate = &gps.detect_state[instance];

    if (dstate->auto_detected_baud) {
        snprintf(buffer, buflen,
                 "GPS %d: detected as %s at %d baud",
                 instance + 1,
                 sensor_gps_backend_name(gps_backend),
                 (int)(gps._baudrates[dstate->current_baud]));
    } else {
        snprintf(buffer, buflen,
                 "GPS %d: specified as %s",
                 instance + 1,
                 sensor_gps_backend_name(gps_backend));
    }
}

static void _bind_function(sensor_gps_backend_t gps_backend, struct sensor_gps_backend_ops* ops)
{
    gps_backend->ops = &gps_backend_ops;

    if (ops->inject_data == NULL) {
        ops->inject_data = gps_backend->ops->inject_data;
    }

    if (ops->send_mavlink_gps_rtk == NULL) {
        ops->send_mavlink_gps_rtk = gps_backend->ops->send_mavlink_gps_rtk;
    }

    if (ops->Write_AP_Logger_Log_Startup_messages == NULL) {
        ops->Write_AP_Logger_Log_Startup_messages = gps_backend->ops->Write_AP_Logger_Log_Startup_messages;
    }

    gps_backend->ops = ops;
}

static void inject_data(sensor_gps_backend_t gps_backend, const uint8_t *data, uint16_t len)
{
    // not all backends have valid ports
    if (gps_backend->port != NULL) {
        if (SerialManager_tx_space(gps_backend->port) > len) {
            rt_device_write(gps_backend->port, 0, data, len);
        } else {
            GPS_BACKEND_DEUBG("GPS %d: Not enough TXSPACE", gps_backend->state->instance + 1);
        }
    }
}

static void send_mavlink_gps_rtk(sensor_gps_backend_t gps_backend, mavlink_channel_t chan)
{
#if 0
    const uint8_t instance = gps_backend->state->instance;
    // send status
    switch (instance) {
        case 0:
            mavlink_msg_gps_rtk_send(chan,
                                 0,  // Not implemented yet
                                 0,  // Not implemented yet
                                 gps_backend->state->rtk_week_number,
                                 gps_backend->state->rtk_time_week_ms,
                                 0,  // Not implemented yet
                                 0,  // Not implemented yet
                                 gps_backend->state->rtk_num_sats,
                                 gps_backend->state->rtk_baseline_coords_type,
                                 gps_backend->state->rtk_baseline_x_mm,
                                 gps_backend->state->rtk_baseline_y_mm,
                                 gps_backend->state->rtk_baseline_z_mm,
                                 gps_backend->state->rtk_accuracy,
                                 gps_backend->state->rtk_iar_num_hypotheses);
            break;
        case 1:
            mavlink_msg_gps2_rtk_send(chan,
                                 0,  // Not implemented yet
                                 0,  // Not implemented yet
                                 gps_backend->state->rtk_week_number,
                                 gps_backend->state->rtk_time_week_ms,
                                 0,  // Not implemented yet
                                 0,  // Not implemented yet
                                 gps_backend->state->rtk_num_sats,
                                 gps_backend->state->rtk_baseline_coords_type,
                                 gps_backend->state->rtk_baseline_x_mm,
                                 gps_backend->state->rtk_baseline_y_mm,
                                 gps_backend->state->rtk_baseline_z_mm,
                                 gps_backend->state->rtk_accuracy,
                                 gps_backend->state->rtk_iar_num_hypotheses);
            break;
    }
#endif
}

static void Write_AP_Logger_Log_Startup_messages(sensor_gps_backend_t gps_backend)
{

}

/*------------------------------------test------------------------------------*/


